module mesh_math_mod

  use mesh_const_mod
  use gauss_quad_mod

  implicit none

  !private

  !public cart_to_latlon
  !public latlon_to_cart
  !public rotate
  !public rotate_back
  !public orient
  !public gaussian_legendre
  !public det3

contains

  subroutine cart_to_latlon(x, y, z, lon, lat)

    real(8), intent(in) :: x
    real(8), intent(in) :: y
    real(8), intent(in) :: z
    real(8), intent(out) :: lon
    real(8), intent(out) :: lat

    lon = atan2(y, x)
    lat = asin(z) ! Assume x**2 + y**2 + z**2 = 1.
    if (lon < 0.0) lon = lon + pi2
    if (lon > pi2) lon = lon - pi2

  end subroutine cart_to_latlon

  subroutine latlon_to_cart(lon, lat, x, y, z)

    real(8), intent(in) :: lon
    real(8), intent(in) :: lat
    real(8), intent(out) :: x
    real(8), intent(out) :: y
    real(8), intent(out) :: z

    real(8) cos_lat

    cos_lat = cos(lat)
    x = cos_lat * cos(lon)
    y = cos_lat * sin(lon)
    z = sin(lat)

  end subroutine latlon_to_cart

  subroutine rotate(lon_np, lat_np, lon, lat, lon_rot, lat_rot)

    real(8), intent(in) :: lon_np
    real(8), intent(in) :: lat_np
    real(8), intent(in) :: lon
    real(8), intent(in) :: lat
    real(8), intent(out) :: lon_rot
    real(8), intent(out) :: lat_rot

    real(8) cos_lat_np, sin_lat_np, cos_lat, sin_lat, cos_dlon, sin_dlon
    real(8) tmp1, tmp2, tmp3

    cos_lat_np = cos(lat_np)
    sin_lat_np = sin(lat_np)
    cos_lat    = cos(lat   )
    sin_lat    = sin(lat   )
    cos_dlon   = cos(lon - lon_np)
    sin_dlon   = sin(lon - lon_np)

    tmp1 = cos_lat * sin_dlon
    tmp2 = cos_lat * sin_lat_np * cos_dlon - cos_lat_np * sin_lat
    lon_rot = atan2(tmp1, tmp2)
    if (lon_rot < 0.0) lon_rot = pi2 + lon_rot

    tmp1 = sin_lat * sin_lat_np
    tmp2 = cos_lat * cos_lat_np * cos_dlon
    tmp3 = tmp1 + tmp2
    tmp3 = min(1.0, max(-1.0, tmp3))
    lat_rot = asin(tmp3)

  end subroutine rotate

  subroutine rotate_back(lon_np, lat_np, lon_rot, lat_rot, lon, lat)

    real(8), intent(in) :: lon_np
    real(8), intent(in) :: lat_np
    real(8), intent(in) :: lon_rot
    real(8), intent(in) :: lat_rot
    real(8), intent(out) :: lon
    real(8), intent(out) :: lat

    real(8) cos_lat_np, sin_lat_np, cos_lon_rot, sin_lon_rot, cos_lat_rot, sin_lat_rot
    real(8) tmp1, tmp2, tmp3

    cos_lat_np = cos(lat_np)
    sin_lat_np = sin(lat_np)
    cos_lon_rot = cos(lon_rot)
    sin_lon_rot = sin(lon_rot)
    cos_lat_rot = cos(lat_rot)
    sin_lat_rot = sin(lat_rot)

    tmp1 = cos_lat_rot * sin_lon_rot
    tmp2 = sin_lat_rot * cos_lat_np + cos_lat_rot * cos_lon_rot * sin_lat_np
    lon = lon_np + atan2(tmp1, tmp2)
    if (lon > pi2) lon = lon - pi2
    if (lon < 0.0) lon = lon + pi2

    tmp1 = sin_lat_rot * sin_lat_np
    tmp2 = cos_lat_rot * cos_lat_np * cos_lon_rot
    tmp3 = tmp1 - tmp2
    tmp3 = min(1.0, max(-1.0, tmp3))
    lat = asin(tmp3)

  end subroutine rotate_back

  pure integer function orient(x1, x2, x3) result(res)

    real(8), intent(in) :: x1(3)
    real(8), intent(in) :: x2(3)
    real(8), intent(in) :: x3(3)

    real(8) det

    det = x3(1) * (x1(2) * x2(3) - x1(3) * x2(2)) - &
          x3(2) * (x1(1) * x2(3) - x1(3) * x2(1)) + &
          x3(3) * (x1(1) * x2(2) - x1(2) * x2(1))

    if (det > 1.0e-16) then
      res = -1 ! Left
    else if (-det > 1.0e-16) then
      res = 1  ! Right
    else
      res = 0  ! On
    end if

  end function orient
  
  subroutine inverse_matrix(A, iA, ierr)

    real(r16), intent(in ) :: A(:,:)
    real(r16), intent(out) :: iA(size(A,1),size(A,1))
    integer  , intent(out) :: ierr

    real(r16) tmp
    integer n, i, j, k
    integer imax(size(A,1)), jmax(size(A,1))

    ierr = 0
    if (size(A, 1) /= size(A, 2)) then
      ierr = 1
      return
    end if

    n = size(A, 1)

    iA = A

    do k = 1, n
      tmp = 0
      do j = k, n
        do i = k, n
          if (abs(iA(i,j)) > tmp) then
            tmp = abs(iA(i,j)); imax(k) = i; jmax(k) = j
          end if
        end do
      end do
      if (tmp + 1 == 1) then
        ierr = 2
        return
      end if
      do j = 1, n
        tmp = iA(k,j); iA(k,j) = iA(imax(k),j); iA(imax(k),j) = tmp
      end do
      do i = 1, n
        tmp = iA(i,k); iA(i,k) = iA(i,jmax(k)); iA(i,jmax(k)) = tmp
      end do
      iA(k,k) = 1.0d0 / iA(k,k)
      do j = 1, n
        if (j /= k) then
          iA(k,j) = iA(k,j) * iA(k,k)
        end if
      end do
      do i = 1, n
        if (i /= k) then
          do j = 1, n
            if (j /= k) then
              iA(i,j) = iA(i,j) - iA(i,k) * iA(k,j)
            end if
          end do
        end if
      end do
      do i = 1, n
        if (i /= k) then
          iA(i,k) = -iA(i,k) * iA(k,k)
        end if
      end do
    end do

    do k = n, 1, -1
      do j = 1, n
        tmp = iA(k,j); iA(k,j) = iA(jmax(k),j); iA(jmax(k),j) = tmp
      end do
      do i = 1, n
        tmp = iA(i,k); iA(i,k) = iA(i,imax(k)); iA(i,imax(k)) = tmp
      end do
    end do

  end subroutine inverse_matrix
    
  ! calculate determinant
  function det(matrix)
    real(r16)            :: det
    real(r16),intent(in) :: matrix(3,3)
    
    det = matrix(1,1)*matrix(2,2)*matrix(3,3) + matrix(1,2)*matrix(2,3)*matrix(3,1) + matrix(1,3)*matrix(2,1)*matrix(3,2)&
        - matrix(1,3)*matrix(2,2)*matrix(3,1) - matrix(1,2)*matrix(2,1)*matrix(3,3) - matrix(1,1)*matrix(2,3)*matrix(3,2)
  end function det
  
  function cot(x)
    real(r_kind) :: cot
    real(r_kind) :: x
    
    cot = 1._r_kind / tan(x)
  end function cot
  
  function csc(x)
    real(r_kind) :: csc
    real(r_kind) :: x
    
    csc = 1._r_kind / sin(x)
  end function csc
  
  function sec(x)
    real(r_kind) :: sec
    real(r_kind) :: x
    
    sec = 1._r_kind / cos(x)
  end function sec
  
  function kronecker_product(n1,a1,n2,a2) result(res)
    integer n1, n2
    real(r_kind) :: a1(n1)
    real(r_kind) :: a2(n2)
    real(r_kind) :: res(n1*n2)
    
    res = pack(matmul(reshape(a1, [n1,1]), reshape(a2, [1,n2])), .true.)
    
  end function kronecker_product
  
end module mesh_math_mod
